import sys
import math
from sokoban import *
from sokoban_main import *

BIG = 1e308;

def manhattan_distance(c1, c2):
	return abs(c1[0] - c2[0]) + abs(c1[1] - c2[1])

def euclidean_distance(c1, c2):
	return math.sqrt(double((c1[0] - c2[0]) * (c1[0] - c2[0]) + (c1[1] - c2[1]) * (c1[1] - c2[1])));
STORE_VISITED_STATES = 1
INT_MAX = 99999
LEFT, RIGHT, UP, DOWN = range(4)

# Performs a Dijkstra's algorithm to find the shortest paths from a position to all
# coordinates on the map.  It computes a list that stores
# the preceding shortest-path action leading from the start to point (x,y).
# That is, a path from the start to any grid point (x,y) can be extracted by
# following the action at (x,y) back to its parent (x',y'), and then
# recursing on (x',y') until the start is reached.  The array is indexed
# at parent[y*width+x] for a point (x,y).  If the action is None, then it is
# either the start node, or unreachable from the start.		
def navigation_search(start, nmap):
	numSteps = [INT_MAX] * (nmap.w * nmap.h)
	parent = [-1] * (nmap.w * nmap.h)
	q = []
	#print 'map obstacles',nmap.obstacles.keys()

	index = nmap.coord_to_index(start)
	numSteps[index] = 0
	q.append(start)
	while q:
		s = q.pop()
		index = nmap.coord_to_index(s)
		s_numSteps = numSteps[index]

		temp = (s[0] + 1, s[1])
		index = nmap.coord_to_index(temp)
		if (not (nmap.is_obstacle(temp))) and (numSteps[index] > s_numSteps + 1):
			parent[index] = LEFT
			numSteps[index] = s_numSteps + 1
			q.append(temp)

		temp = (s[0] - 1, s[1])
		index = nmap.coord_to_index(temp)
		if (not (nmap.is_obstacle(temp))) and (numSteps[index] > s_numSteps + 1):
			parent[index] = RIGHT
			numSteps[index] = s_numSteps + 1
			q.append(temp)

		temp = (s[0], s[1] + 1)
		index = nmap.coord_to_index(temp)
		if (not (nmap.is_obstacle(temp))) and (numSteps[index] > s_numSteps + 1):
			parent[index] = UP
			numSteps[index] = s_numSteps + 1
			q.append(temp)

		temp = (s[0], s[1] - 1)
		index = nmap.coord_to_index(temp)
		if (not (nmap.is_obstacle(temp))) and (numSteps[index] > s_numSteps + 1):
			parent[index] = DOWN
			numSteps[index] = s_numSteps + 1
			q.append(temp)

	for i in range(len(numSteps)):
		if numSteps[i] == INT_MAX:
			numSteps[i] = -1

	#print 'navigation_search',start,numSteps #for debugging
	return (numSteps, parent)


# Same to navigation_distance except that this is for block (boxes).

# In order for a block to move, it has free space on both side, i.e. left-right, up-down.
# One free space is the target location, the other free space is for player.		
def block_navigation_search(start, nmap):
	numSteps = [INT_MAX] * (nmap.w * nmap.h)
	parent = [-1] * (nmap.w * nmap.h)
	q = []

	index = nmap.coord_to_index(start)
	numSteps[index] = 0
	q.append(start)
	while q:
		s = q.pop()
		index = nmap.coord_to_index(s)
		s_numSteps = numSteps[index]

		a = (s[0] - 1, s[1])
		b = (s[0] + 1, s[1])
		if (not nmap.is_obstacle(a)) and (not nmap.is_obstacle(b)):
			index = nmap.coord_to_index(a)
			if (numSteps[index] > s_numSteps + 1):
				parent[index] = RIGHT
				numSteps[index] = s_numSteps + 1
				q.append(a)
			index = nmap.coord_to_index(b)
			if (numSteps[index] > s_numSteps + 1):
				parent[index] = LEFT
				numSteps[index] = s_numSteps + 1
				q.append(b)

		a = (s[0], s[1] - 1)
		b = (s[0], s[1] + 1)
		if (not nmap.is_obstacle(a)) and (not nmap.is_obstacle(b)):
			index = nmap.coord_to_index(a)
			if (numSteps[index] > s_numSteps + 1):
				parent[index] = DOWN
				numSteps[index] = s_numSteps + 1
				q.append(a)
			index = nmap.coord_to_index(b)
			if (numSteps[index] > s_numSteps + 1):
				parent[index] = UP
				numSteps[index] = s_numSteps + 1
				q.append(b)

	for i in range(len(numSteps)):
		if numSteps[i] == INT_MAX:
			numSteps[i] = -1

	#print 'navigation_search',start,numSteps #for debugging
	return (numSteps, parent)

#===============================================================================
# The following are several methods that help heuristic methods in the
# SokobanHeuristic class.
#
# Chao Sun (chaosun)
#===============================================================================

# method for calculating permutations of the elements of a list
# code from http://snippets.dzone.com/posts/show/753
def all_perms(str):
	if len(str) <= 1:
		yield str
	else:
		for perm in all_perms(str[1:]):
			for i in range(len(perm) + 1):
				yield perm[:i] + str[0:1] + perm[i:]

# navigation_distance for navigation_heuristic
def navigation_distance(p_navdis, objs, goals, po, pg, map):
 	o_dis = 0
 	g_dis = 0

	for i in range(len(objs)):
		obj = objs[po[i]]
		o_index = map.coord_to_index(obj)
		# for the first obj, get the steps from player to it
		if (i == 0):
			if (p_navdis[o_index] < 0):
				return 99999
			else:
				o_dis = p_navdis[o_index]
		# for other objs, the steps from the last goal to it
		else:
			f_goal = goals[pg[i - 1]]
			f_g_nav_dis = navigation_search(f_goal, map)
			f_g_navdis = f_g_nav_dis[0]
			if (f_g_navdis[o_index] < 0):
				return 99999
			else:
				o_dis += f_g_navdis[o_index]
		# for each goal, the steps from the corresponding obj to it
		o_nav_dis = block_navigation_search(obj, map)
		o_navdis = o_nav_dis[0]
		goal = goals[pg[i]]
		g_index = map.coord_to_index(goal)
		if (o_navdis[g_index] < 0):
			return 99999
		else:
			g_dis += o_navdis[g_index]

	return g_dis + o_dis

# cashed_search for block_navigation_heuristic
def cashed_search(cache, start, end, type, map):
	# if start and end points in cache, just retrieve the value
	for e in cache:
		if (e[0] == start) and (e[1] == end) and (e[2] == type):
			return e[3]

	# if not, call navigation functions due to the type
	if type == 0:
		nav_dis = navigation_search(start, map)
	else:
		nav_dis = block_navigation_search(start, map)
	navdis = nav_dis[0]
	steps = navdis[map.coord_to_index(end)]

	if steps < 0:
		steps = 99999

	# cache the info, return the number of steps
	cache.append([start, end, type, steps])
	return steps

# block_nav_dis for block_navigation_heuristic
def block_nav_dis(p_navdis, start, objs, goals, po, pg, map, cache):
	o_dis = 0
 	g_dis = 0

	for i in range(len(objs)):
		obj = objs[po[i]]
		# for the first obj, get the steps from player to it
		if (i == 0):
			flag = 0
			# if in the cache, retrieve it and reset the flag
			for e in cache:
				if (e[0] == start) and (e[1] == obj):
					o_index = e[3]
					flag = 1
			# if flag is not reset, calculate steps and append info. to cache
			if flag == 0:
				o_index = map.coord_to_index(obj)
				if (p_navdis[o_index] < 0):
					cache.append([start, obj, 0, 99999])
					return 99999
				else:
					o_dis = p_navdis[o_index]
					cache.append([start, obj, 0, o_dis])
		# for other objs, the steps from the last goal to it
		else:
			f_goal = goals[pg[i - 1]]
			fg_o = cashed_search(cache, f_goal, obj, 0, map)
			if (fg_o < 0):
				return 99999
			else:
				o_dis += fg_o
		# for each goal, the steps from the corresponding obj to it
		goal = goals[pg[i]]
		o_g = cashed_search(cache, obj, goal, 1, map)
		if (o_g < 0):
			return 99999
		else:
			g_dis += o_g

	return g_dis + o_dis

#===============================================================================
# IMPLEMENT ME!
# 
# Each of the following ___Heuristic() methods (with the exception of
# NullHeuristic() should be implemented.
#
# Chao Sun (chaosun)
#===============================================================================
class SokobanHeuristic:
	def __init__(self, smap):
		self.smap = smap
		self.navMap = NavigationMap()
		self.navMap.w = smap.w
		self.navMap.h = smap.h
		self.navMap.obstacles = smap.obstacles
		self.cached_shortest_paths = [0] * (smap.w * smap.h)
		self.cached_shortest_paths_block = [0] * (smap.w * smap.h)
		self.shortest_distance = [0] * (smap.w * smap.h)
		self.shortest_distance_block = [0] * (smap.w * smap.h)
		# Cache for the former heuristic results of the steps between two points.
		# Info. in each element: start and end points; number of steps; type of
		# heuristic.
		self.cache = []

	def null_heuristic(self, state):
		return 0

	def manhattan_heuristic(self, state):
		# return value set as 99999 initially
		man_dis = 99999

		# read objects and goals
		objs = state.objects
		goals = self.smap.goals.keys()

		# enumerate permutation of objs and goals
		pn1 = list(all_perms(range(len(objs))))
		pn2 = list(all_perms(range(len(goals))))

		# for each possible obj-goal combination, do the heuristic by
		# summing up the distances between two points along the order
		# we go from the player to every object and goals, enumerating
		# all the possible permutations
		for po in pn1:
			for pg in pn2:
				o_g_dis = 0
				for i in range(len(objs)):
					o_g_dis += manhattan_distance(objs[po[i]], goals[pg[i]])
				# take the smallest value to return
				if (man_dis > o_g_dis):
					man_dis = o_g_dis

		return man_dis

	# this method calls the navigation_distance method
	def navigation_heuristic(self, state):
		# return value set as 99999 initially
		tot_dis = 99999

		# navigation result from player coordinate
		start = state.playerCoord
		map = self.navMap
		p_nav_dis = navigation_search(start, map)
		p_navdis = p_nav_dis[0]

		# read objects and goals
		objs = state.objects
		goals = self.smap.goals.keys()

		# enumerate permutations of objs and goals
		pn1 = list(all_perms(range(len(objs))))
		pn2 = list(all_perms(range(len(goals))))

		# for each possible obj-goal combination, do the heuristic
		for po in pn1:
			for pg in pn2:
				distance = navigation_distance(p_navdis, objs, goals, po, pg, map)
				# choose the smallest value to return
				if distance < tot_dis:
					tot_dis = distance

		return tot_dis

	def cached_navigation_heuristic(self, state):
		# return value set as 99999 initially
		tot_dis = 99999

		# navigation result from player coordinate
		start = state.playerCoord
		map = self.navMap
		p_nav_dis = navigation_search(start, map)
		p_navdis = p_nav_dis[0]

		# read objects and goals
		objs = state.objects
		goals = self.smap.goals.keys()

		# enumerate permutations of objs and goals
		pn1 = list(all_perms(range(len(objs))))
		pn2 = list(all_perms(range(len(goals))))

		# for each possible obj-goal combination, do the heuristic
		for po in pn1:
			for pg in pn2:
				distance = block_nav_dis(p_navdis, start, objs, goals, po, pg, map, self.cache)
				# choose the smallest value to return
				if distance < tot_dis:
					tot_dis = distance

		return tot_dis

	def other_heuristic(state):
		return 0
